
#include <QApplication>
#include <QtGui>
#include <thread>
#include <chrono>
#include "include/mainwindow.h"
#include "include/Laser.h"
#include "include/Astar_Right.h"


bool hasSamePoint(vector<point> a, vector<point> b) {
    for (auto &pa: a) {
        for (auto &pb: b) {
            if (pa == pb) {
                return true;
            }
        }
    }
    return false;
}

int func(void *p) {
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    MainWindow *mainWin = (MainWindow *) p;
    RMap rmap;
    rmap.load("/home/l/input.txt");


    mainWin->initMap(rmap.m_map, rmap.m_line, rmap.m_col);


    Laser laser(1, 5);

    point current(33, 73);

    Astar a(rmap.m_line, rmap.m_col);
    a.map = rmap.m_map;
    a.search(new point(current.getx(), current.gety()), new point(rmap.m_endX, rmap.m_endY));
    vector<point> path = a.path;

    bool flag = true;
    int old_x = 0;
    int old_y = 0;
    mainWin->drawPoint(rmap.m_startX, rmap.m_startY, Qt::red);

    mainWin->drawPoint(rmap.m_endX, rmap.m_endY, Qt::blue);
    vector<point> old_path;
    while (flag) {


        Astar a(rmap.m_line, rmap.m_col);
        a.map = rmap.m_map;
        a.search(new point(current.getx(), current.gety()), new point(rmap.m_endX, rmap.m_endY));
        cout << "find the path_temp ";

        vector<point> path_temp = a.path;
        //清除路径
        for (auto &po: old_path) {
            if (rmap.m_map[po.getx()][po.gety()] != RMap::RMAP_OBSTACLE_SCANNED &&
                rmap.m_map[po.getx()][po.gety()] != RMap::RMAP_OBSTACLE_UNDESCOVERED) {
                mainWin->clearPoint(po.getx(), po.gety());
            }
        }
        //打点路径
        for (auto &po: path_temp) {
            if (rmap.m_map[po.getx()][po.gety()] != RMap::RMAP_OBSTACLE_SCANNED &&
                rmap.m_map[po.getx()][po.gety()] != RMap::RMAP_OBSTACLE_UNDESCOVERED) {
                mainWin->drawPoint(po.getx(), po.gety(), Qt::green);
            }
        }
        mainWin->drawPoint(rmap.m_startX, rmap.m_startY, Qt::red);

        mainWin->drawPoint(rmap.m_endX, rmap.m_endY, Qt::green);


        for (int i = 0; i < path_temp.size(); i++) {


            point cur_p = path_temp[i];

            cout << "current pos is " << cur_p.getx() << " " << cur_p.gety() << endl;

            current.x = cur_p.getx();
            current.y = cur_p.gety();

            mainWin->clearPoint(old_x, old_y);

            mainWin->drawPoint(current.getx(), current.gety(), Qt::yellow);


            old_x = current.x;
            old_y = current.y;

            vector<point> radar = laser.scan(rmap, current.getx(), current.gety());


            if (current.x == rmap.m_endX && current.y == rmap.m_endY) {
                flag = false;
                break;
            }

            for (auto &po: radar) {
                mainWin->drawPoint(po.getx(), po.gety(), Qt::red);
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            //雷达打点
            if (!radar.empty()) {
                rmap.pointMap(radar, (MainWindow *) mainWin);
                if (hasSamePoint(radar, path_temp)) {
                    break;
                }
            }


        }
        old_path = path_temp;
    }

    return 0;
}


int main(int argc, char *argv[]) {

    RMap rmap;
    rmap.load("../input.txt");

    QApplication app(argc, argv);

    MainWindow mainWin;

    mainWin.drawGrid();

    mainWin.show();

    //在Mainwindow.cpp或者mainwindow.h包含上面的.h会编译错误
    std::thread th(func,&mainWin);

    return QApplication::exec();
}
